

#include "narf.h"

typedef PointXYZ PointType;

void
narf (PointCloud<PointXYZ>::Ptr point_cloud_ptr)
{
    float angular_resolution = deg2rad(0.1f);
    float support_size = 20.0f;
    RangeImage::CoordinateFrame coordinate_frame = RangeImage::CAMERA_FRAME;
    bool setUnseenToMaxRange = false;
  // ------------------------------------------------------------------
  // -----Read pcd file or create example point cloud if not given-----
  // ------------------------------------------------------------------
  PointCloud<PointType>& point_cloud = *point_cloud_ptr;
  PointCloud<PointWithViewpoint> far_ranges;
  Eigen::Affine3f scene_sensor_pose (Eigen::Affine3f::Identity ());


//loadCloud("/home/stefano/edmond/scans/lamiera3d/08/points.pcd",point_cloud_ptr,1,1,1);

PointXYZ minCoord=point_cloud.at(0);
PointXYZ maxCoord=point_cloud.at(0);
for (int i=1;i<point_cloud.size();i++)
{
    if(point_cloud.at(i).x < minCoord.x && point_cloud.at(i).y < minCoord.y)  minCoord=point_cloud.at(i);
    if(point_cloud.at(i).x > maxCoord.x && point_cloud.at(i).y > maxCoord.y)  maxCoord=point_cloud.at(i);
}

PointXYZ center;
center.x=(maxCoord.x+minCoord.x)/2;
center.y=(maxCoord.y+minCoord.y)/2;
cout << center.x << " and "<<center.y<<endl;

    scene_sensor_pose = Eigen::Affine3f (Eigen::Translation3f (/*point_cloud.sensor_origin_[0],
                                                               point_cloud.sensor_origin_[1],
                                                               point_cloud.sensor_origin_[2]*/ center.x,center.y,0.0f)) *
                        Eigen::Affine3f (point_cloud.sensor_orientation_);
  // -----------------------------------------------
  // -----Create RangeImage from the PointCloud-----
  // -----------------------------------------------
  float noise_level = 0.0;
  float min_range = 0.0f;
  int border_size = 1;
  boost::shared_ptr<RangeImage> range_image_ptr (new RangeImage);
  RangeImage& range_image = *range_image_ptr;
  range_image.createFromPointCloud (point_cloud, angular_resolution, deg2rad (360.0f), deg2rad (180.0f),
                                   scene_sensor_pose, coordinate_frame, noise_level, min_range, border_size);
  range_image.integrateFarRanges (far_ranges);
  if (setUnseenToMaxRange)
    range_image.setUnseenToMaxRange ();

  // --------------------------------------------
  // -----Open 3D viewer and add point cloud-----
  // --------------------------------------------
  visualization::PCLVisualizer viewer ("3D Viewer");
  viewer.setBackgroundColor (1, 1, 1);
  //visualization::PointCloudColorHandlerCustom<PointWithRange> range_image_color_handler (range_image_ptr, 0, 0, 0);
  //viewer.addPointCloud (range_image_ptr, range_image_color_handler, "range image");
  //viewer.setPointCloudRenderingProperties (visualization::PCL_VISUALIZER_POINT_SIZE, 1, "range image");
  //viewer.addCoordinateSystem (1.0f);
  visualization::PointCloudColorHandlerCustom<PointType> point_cloud_color_handler (point_cloud_ptr, 150, 150, 150);
  viewer.addPointCloud (point_cloud_ptr, point_cloud_color_handler, "original point cloud");
  viewer.initCameraParameters ();

  // --------------------------
  // -----Show range image-----
  // --------------------------
  visualization::RangeImageVisualizer range_image_widget ("Range image");
  range_image_widget.showRangeImage (range_image);

  // --------------------------------
  // -----Extract NARF keypoints-----
  // --------------------------------
  RangeImageBorderExtractor range_image_border_extractor;
  NarfKeypoint narf_keypoint_detector (&range_image_border_extractor);
  narf_keypoint_detector.setRangeImage (&range_image);
  narf_keypoint_detector.getParameters ().support_size = support_size;
  //narf_keypoint_detector.getParameters ().add_points_on_straight_edges = true;
  //narf_keypoint_detector.getParameters ().distance_for_additional_points = 0.5;

  PointCloud<int> keypoint_indices;
  narf_keypoint_detector.compute (keypoint_indices);
  std::cout << "Found "<<keypoint_indices.points.size ()<<" key points.\n";

  // ----------------------------------------------
  // -----Show keypoints in range image widget-----
  // ----------------------------------------------
  //for (size_t i=0; i<keypoint_indices.points.size (); ++i)
    //range_image_widget.markPoint (keypoint_indices.points[i]%range_image.width,
                                  //keypoint_indices.points[i]/range_image.width);

  // -------------------------------------
  // -----Show keypoints in 3D viewer-----
  // -------------------------------------
  PointCloud<PointXYZ>::Ptr keypoints_ptr (new PointCloud<PointXYZ>);
  PointCloud<PointXYZ>& keypoints = *keypoints_ptr;
  keypoints.points.resize (keypoint_indices.points.size ());
  for (size_t i=0; i<keypoint_indices.points.size (); ++i)
    keypoints.points[i].getVector3fMap () = range_image.points[keypoint_indices.points[i]].getVector3fMap ();

  visualization::PointCloudColorHandlerCustom<PointXYZ> keypoints_color_handler (keypoints_ptr, 0, 255, 0);
  viewer.addPointCloud<PointXYZ> (keypoints_ptr, keypoints_color_handler, "keypoints");
  viewer.setPointCloudRenderingProperties (visualization::PCL_VISUALIZER_POINT_SIZE, 7, "keypoints");

  //--------------------
  // -----Main loop-----
  //--------------------
  while (!viewer.wasStopped ())
  {
    range_image_widget.spin ();  // process GUI events
    viewer.spin ();
  }
}
